#include "livox.h"
// #include "livox_m.h"
#include "pcl/visualization/cloud_viewer.h"
#include <LdPcl.h>
// #include "livox.h"

int main()
{
    Eigen::Matrix4f CL_RoT;
    Eigen::Matrix3f CamIntrix;
    int PointsNum;
    int Img_W, Img_H;
    int CloudsNum;
    cv::FileStorage LoadMycfg_main("../YmlFiles/Dahua.yml", cv::FileStorage::READ);
    LoadMycfg_main["fx"] >> CamIntrix(0, 0);
    LoadMycfg_main["cx"] >> CamIntrix(0, 2);
    LoadMycfg_main["px"] >> CamIntrix(0, 1);
    LoadMycfg_main["fy"] >> CamIntrix(1, 1);
    LoadMycfg_main["cy"] >> CamIntrix(1, 2);

    LoadMycfg_main["LC00"] >> CL_RoT(0, 0);
    LoadMycfg_main["LC01"] >> CL_RoT(0, 1);
    LoadMycfg_main["LC02"] >> CL_RoT(0, 2);
    LoadMycfg_main["LC03"] >> CL_RoT(0, 3);
    LoadMycfg_main["LC10"] >> CL_RoT(1, 0);
    LoadMycfg_main["LC11"] >> CL_RoT(1, 1);
    LoadMycfg_main["LC12"] >> CL_RoT(1, 2);
    LoadMycfg_main["LC13"] >> CL_RoT(1, 3);
    LoadMycfg_main["LC20"] >> CL_RoT(2, 0);
    LoadMycfg_main["LC21"] >> CL_RoT(2, 1);
    LoadMycfg_main["LC22"] >> CL_RoT(2, 2);
    LoadMycfg_main["LC23"] >> CL_RoT(2, 3);
    LoadMycfg_main["PointsNum"] >> PointsNum;
    LoadMycfg_main["CloudsNum"] >> CloudsNum;
    LoadMycfg_main["Width"] >> Img_W;
    LoadMycfg_main["Height"] >> Img_H;
    LoadMycfg_main.release();

    std::vector<std::string> BroadCode;
    char saved_path[100];
    // std::string saved;
    // BroadCode.push_back("000000000000001");
    
    //创建雷达线程，开启雷达
    LdsLidar &mylidar = LdsLidar::GetInstance();
    BroadCode.push_back("3WEDH7600110891");
    mylidar.InitLdsLidar(BroadCode, PointsNum, CloudsNum);
    // mylidar.InitLdsLidar("auto", 100);
    sleep(1);
    pcl::visualization::CloudViewer viewerAll("Viewer All");
    cv::namedWindow("DepthImg", cv::WINDOW_FREERATIO);
   
    int i = 0;

    // while (!viewerAll.wasStopped()&& !viewer1.wasStopped() && !viewer2.wasStopped() && !viewer3.wasStopped() && !viewer4.wasStopped() && !viewer5.wasStopped())
    // while(!viewerAll.wasStopped())
    while (!viewerAll.wasStopped())
    // while(true)
    {
        std::vector<CloudI> clouds;
        std::vector<Gydata> Gys
        cv::Mat DepthImg = cv::Mat::zeros(Img_H, Img_W, CV_16UC1);
        clouds.resize(CloudsNum);
        Gys.resize(CloudsNum);
        mylidar.getall(clouds, Gys);
        PcloudI dest(new CloudI());
        *dest = clouds[CloudsNum-1];
        timeval time;
        double starttime, endtime;
        gettimeofday(&time, NULL);
        starttime = time.tv_sec * 1e3 + time.tv_usec * 1e-3;
        for (size_t i = 0; i < CloudsNum; i++)
        {
            Eigen::Matrix4f rot;
            Gydata2RoT(Gys[i], Gys[CloudsNum-1], rot);
            std::cout << rot << std::endl;
            Fusion(clouds[i], dest, rot);
            // Fusion(clouds[i], DepthImg, CL_RoT, CamIntrix, rot);
        }
        viewerAll.showCloud(dest);
        gettimeofday(&time, NULL);
        endtime = time.tv_sec * 1e3 + time.tv_usec * 1e-3;
        std::cout << "融合帧率为: " << 1000 / (endtime - starttime) << std::endl;
        // cv::Mat destImg = cv::Mat::zeros(1024, 1280, CV_8UC3);
        // cv::cvtColor(DepthImg, destImg, cv::COLORMAP_HOT);
        // // cv::applyColorMap(DepthImg, destImg, cv::COLORMAP_HSV);
        // cv::imshow("DepthImg", destImg);
        // cv::waitKey(1);
        // PcloudI origin(new CloudI());
        // Gydata gy;
        // mylidar.getdata(origin, gy);
        // viewerAll.showCloud(origin);
        // continue;
        sprintf(saved_path, "/home/milo/Pictures/pcd/4.30/ronghe%d.pcd", i++);
        // std::vector<Gydata> Gys;
        // std::vector<CloudI> Clouds;
        // Gys.resize(5);
        // Clouds.resize(5);
        //从雷达中连续获取5帧点云
        // for (size_t i = 0; i < 5; i++)
        // {

        // PcloudI tempCloud(new CloudI());
        // Gydata tempGy;
        // mylidar.Get5Cloud(Clouds, Gys);
        // PcloudI temp(new CloudI());
        // *temp = Clouds[0];
        // viewerAll.showCloud(temp);
        // Clouds.push_back(tempCloud);
        // Gys.push_back(tempGy);
        // }

        //将5帧点云往最后一帧进行融合
        /*
        PcloudI DestCloud(new CloudI());
        *DestCloud = Clouds[4];
        for (size_t i = 0; i < 4; i++)
        {
            if (Clouds[i].empty() && Clouds[i].size() == 0)
                continue;

            Eigen::Matrix4f RoT;
            Gydata2RoT(Gys[i], Gys[4], RoT);
            // PcloudI temp(new CloudI[Clouds[i]]);

            Fusion(Clouds[i], DestCloud, RoT);
        }

        if (DestCloud->points.size() == 0)
            continue;
        */

        dest->width = dest->points.size();
        dest->height = 1;
        dest->is_dense = false;
        pcl::io::savePCDFile(saved_path, *dest);

        // viewerAll.showCloud(DestCloud);
        // while(!viewer.wasStopped()){}

        // std::cout << "Tran is: " << RoT.Tran << std::endl;
        // std::cout << "yaw is: " << RoT.yaw << std::endl;
    }
    return 0;
}